//
//  Quaternions.c
//  3DMathSDK
//
//  Created by admin on 16/7/12.
//  Copyright © 2016年 douyu. All rights reserved.
//

#include "Quaternions.h"
#include <math.h>
#include "Vector3.h"

const Quaternions kQuaternionsZero = (Quaternions){0, {0, 0, 0}};

bool isQuaternionsEqual(Quaternions q1, Quaternions q2)
{
    if (q1.w == q2.w && isVectorEqual(q1.v, q2.v)) {
        true;
    }
    return false;
}

Quaternions QuaternionsMake(float w, float x, float y, float z)
{
    Quaternions q = {w, (Vector3){x, y, z}};
    return q;
}

Quaternions rotateQuaternions(Vector3 vector, float angle)
{
    return QuaternionsMake(cosf(angle), sinf(angle)*vector.x, sinf(angle)*vector.y, sinf(angle)*vector.z);
}

float moduloQuaternions(Quaternions q)
{
    return sqrtf(powf(q.w, 2)+powf(modulo(q.v), 2));
}

Quaternions inverseQuaternions(Quaternions q)
{
    Vector3 conjugate = multiplyNum(q.v, -1.f); // 共轭
    float modulo = moduloQuaternions(q);
    Quaternions result = QuaternionsMake(q.w/modulo, conjugate.x/modulo, conjugate.y/modulo, conjugate.z/modulo);
    return result;
}

Quaternions crossProductQuaternions1(Quaternions q1, Quaternions q2)
{
    float w = q1.w*q2.w-dotProduct(q1.v, q2.v);
    Vector3 v = plus(plus(multiplyNum(q2.v, q1.w), multiplyNum(q1.v, q2.w)), crossProduct(q2.v, q1.v));
    return QuaternionsMake(w, v.x, v.y, v.z);
}

Quaternions crossProductQuaternions(Quaternions q1, Quaternions q2)
{
    float w = q1.w*q2.w-dotProduct(q1.v, q2.v);
    Vector3 v = plusNum(plus(multiplyNum(q2.v, q1.w), multiplyNum(q1.v, q2.w)), dotProduct(q1.v, q2.v));
    return QuaternionsMake(w, v.x, v.y, v.z);
}

Quaternions displacementQuaternions(Quaternions q1, Quaternions q2)
{
    Quaternions inverse = inverseQuaternions(q1);
    return crossProductQuaternions(inverse, q2);
}

float dotProductQuaternions(Quaternions q1, Quaternions q2)
{
    return q1.w*q2.w + dotProduct(q1.v, q2.v);
}

Quaternions powQuaternions(Quaternions q, float exponent)
{
    Quaternions result = q;
    if (fabsf(exponent) < .9999f) {
        float alpha = acosf(q.w);
        float newAlpha = alpha*exponent;
        result.w = cosf(newAlpha);
        float mult = sinf(newAlpha)/sinf(alpha);
        result.v.x *= mult;
        result.v.y *= mult;
        result.v.z *= mult;
    } else {
        return kQuaternionsZero;
    }
    return result;
}

Quaternions slerp(Quaternions q1, Quaternions q2, float t)
{
    Quaternions q1Inverse = inverseQuaternions(q1);
    Quaternions displacement = displacementQuaternions(q1Inverse, q2);
    Quaternions powQua = powQuaternions(displacement, t);
    return displacementQuaternions(q1, powQua);
}
